/*
 * Copyright (C) 2010 Johan Waldeback
 *  
 * This file is part of the OpenFMS project (http://www.openfms-project.org)
 * 
 * OpenFMS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * \class MockModel
 * \brief Mock simulator class
 *
 * A mock simulator object with a predefined behaviour.
 * Intended to used when testing components that uses a simulator class.
 */

#include "MockModel.h"
#include <cmath>
#include <cassert>
using namespace OpenFms;

/**
 * Mock create function
 *
 * does nothing, always succeeds
 *
 * @param readfn - not used
 * @param custom - not used
 * @return Alwys returns true
 */
bool MockModel::create(ModelSimulator_read_callback_t readfn, uint64_t custom)
{
	// always succeed
	return true;
}

/**
 * Mock destroy function
 *
 * does nothing
 */
void MockModel::destroy()
{
}

/**
 * Mock setStrpSize function
 *
 * Does nothing, always succeeds.
 *
 * @param stepSize - not used.
 * @return Always returns true.
 */
bool MockModel::setStepSize(float stepSize)
{
	return true;
}

/**
 * Mock getInertias function
 *
 * Fills in the inertias structure in a predefined way:
 * If inertias.size < 20 -> return false, otherwise return true
 * The values (if the inertias.size is large enough, is set to:
 * mass = 1, xx = 11, xy = 12, xz =13, yy = 22, yz = 23
 *
 * @param inertias data structure to be filled in
 * @return Returns true on success, false on fail.
 */
bool MockModel::getInertias(ModelSimulator_inertias_t &inertias)
{
	assert( inertias.size >= 20);
	// TODO: remove the following restriction:
	assert( inertias.size == 20 || inertias.size == 24 || inertias.size == 28 || inertias.size == 32 );

	if(inertias.size < 20 ) return false;
	inertias.mass = 1.f;
	inertias.ixx = 11.f;
	inertias.iyy = 22.f;
	inertias.izz = 33.f;
	if( inertias.size >= 24 ) inertias.ixy = 12.f;
	if( inertias.size >= 28 ) inertias.iyz = 13.f;
	if( inertias.size >= 32 ) inertias.iyz = 23.f;
	return true;
}

/**
 * Mock step function
 *
 * Fills in the forces structure in a predefined way:
 * If environment.size != 80 or forces!=28 -> return false, otherwise return true
 * All values is in the forces structure are set to 0, with the following exeptions:
 *
 * force_x is set to the value 0.123f;
 * force_y is set to the value specified by the channel 1 
 * force_z is set to sin ( PI * channel 2 ) (i.e. [-1,1] => [ sin(-PI), sin(PI) ]
 * torque_x is set to the value wind_x in the environment structure
 * torque_y is set to the value wind_y in the environment structure
 * torque_z is set to the value wind_z in the environment structure
 *
 * @param[in] environment data structure 
 * @param[out] forces data structure to be filled in
 * @return Returns true on success, false on fail.
 */
bool MockModel::step(const ModelSimulator_environment_t &environment, ModelSimulator_forces_t &forces)
{
	if( environment.size != 80 || forces.size != 28 ) return false;

	const float _pi = 2*acos(0.f);
	forces.force_x = 0.123f;
	forces.force_y = environment.channel[0];
	forces.force_z = sin(environment.channel[1]*_pi);
	forces.torque_x = environment.linearVelocityX;
	forces.torque_y = environment.linearVelocityY;
	forces.torque_z = environment.linearVelocityZ;

	return true;
}




